package com.grt192.deploy;

import com.grt192.spot.actuator.GRTDemoLED;
import com.grt192.core.GRTRobot;
import com.grt192.spot.mechanism.hauntedHouse.StatusLight;
import com.sun.spot.resources.transducers.IAnalogInput;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.service.BootloaderListenerService;
import java.io.IOException;

/**
 * This is a bare version of grtframework + WPI Libraries.
 * @author ajc
 */
public class MainRobot extends GRTRobot {

    public void startRobot() {
        BootloaderListenerService.getInstance().start();
        log("StartRobot");
    }

    //test if the voltages are exactly the same
    void gyroVoltageTest() {
        IAnalogInput a0 = EDemoBoard.getInstance().getAnalogInputs()[0];
        IAnalogInput a1 = EDemoBoard.getInstance().getAnalogInputs()[1];
        while (true) {
            double x0 = 0, x1 = 0;
            try {
                x0 = a0.getVoltage();
                x1 = a1.getVoltage();
            } catch (IOException ex) {
                ex.printStackTrace();
            }
            log("" + x0 * 4.5 + "\t" + x1);
        }
    }

    /**
     * PID turning for 90deg.
     */
    public void turn90Deg() {
        StatusLight overall = new StatusLight(new GRTDemoLED(0));
        overall.rawColor(GRTDemoLED.Color.ORANGE);
        //TODO complete
    }
}
